#include "task/command_process.h"

using namespace task;

CommandProcess::CommandProcess(task::TaskProcess* task)
{
    ptr_task_process_.reset(task);
    ptr_task_handle_ = std::make_shared<task::LocalHost>();
}

CommandProcess::~CommandProcess()
{
    ptr_task_process_.release();
}

void CommandProcess::Init(std::string &path)
{
    // printf("CommandProcess::Init() path: %s\n", path.c_str());
    ptr_task_handle_->Init(path);

    YAML::Node vehicle_params = YAML::LoadFile(path);
    YAML::Node control_params = vehicle_params["amr_control"];

    if (control_params["cfc_max_speed_toward_degree"].IsDefined())
    {
        cfc_max_speed_toward = control_params["cfc_max_speed_toward_degree"].as<float>()*M_PI/180.0f;
    }
    else
    {
        cfc_max_speed_toward = 95.0f*M_PI/180.0f;
    }
    printf("task::CommandProcess::Init() --> %f\n", cfc_max_speed_toward);
}

void CommandProcess::TaskCmdProcess(task::sAMRState &amr_state, task::sTaskMsg &task_msg)
{
    task::sTaskReceive task_receive;
    ptr_task_handle_->ReadData(task_receive);
    
    if (task_receive.header == "task")
    {
        ptr_task_handle_->WriteData("confirm_task", amr_state, task_receive.SN);
        if (task_receive.command == "working" && !task_receive.point_list.empty())
        {
            PathExtraction(task_receive.point_list, task_receive.task_id, task_receive.speed_level, task_msg);
            task_msg.target_point_id = task_receive.target_id; //TODO
            task_msg.task_id = task_receive.task_id;
            
            if (task_receive.point_list.size() == 1)
            {
                task_msg.v_task_path[0].v_key_points[0].v_work_task = task_receive.work_task;
            }
        }
    }
    LINFO("CommandProcess::TaskCmdProcess() size: {}", task_msg.v_task_path.size());
}

bool CommandProcess::PathExtraction(std::vector<std::string> &point_sequence, uint32_t task_id, float speed_level, task::sTaskMsg &task_msg)
{
    std::vector<std::string>::iterator it = point_sequence.begin();
    while (it!=point_sequence.end()-1)
    {
        map_operation_msgs::GetPathInfo srv;
        srv.request.mode = 1;
        srv.request.start = *it;
        srv.request.end   = *(it+1);

        if (!ptr_task_process_->GetPathInofSrv(srv))
        {
            // printf("start: %s, end: %s\n", (*it).c_str(), (*(it+1)).c_str());
            return false;
        }

        task::sTaskPath path;
        path.path_id = srv.response.rpathid;
        path.max_speed = std::min((float)srv.response.max_speed, speed_level);
        path.task_id = task_id;
        path.path_type = srv.response.pathtype;
        path.angle_type = srv.response.angle_type;
        path.amr_angle = srv.response.angle;
        
        sTaskPoint point;
        if (ptr_task_process_->GetPointInfoSrv(*it, point))
        {
            path.v_control_points.push_back(point);
        }
        if (ptr_task_process_->GetPointInfoSrv(*(it+1), point))
        {
            path.v_control_points.push_back(point);
        }

        if (*it == srv.response.key_points[0].pointid)
        {
            path.move_direction = "forward";
        }
        else
        {
            path.move_direction = "backward";
        }
        
        if (path.angle_type == "shift" && path.path_type == "line")
        {
            float line_theta = atan2(srv.response.key_points[1].pose.position.y-srv.response.key_points[0].pose.position.y, 
                                     srv.response.key_points[1].pose.position.x-srv.response.key_points[0].pose.position.x);

            float dtheta = com_.AngleConstraintToPI(line_theta - path.amr_angle);
            printf("Shift line_theta: %f, dtheta: %f\n", line_theta, dtheta);
            if (fabs(dtheta) - (95*M_PI/180.0f) > FLT_EPSILON)
            {
                path.move_direction = (path.move_direction=="forward")?"backward":"forward";
            }
        }
        path.v_key_points = path.v_control_points;// TODO

        task_msg.v_task_path.push_back(path);
        
        it++;
    }

    if (point_sequence.size() == 1)
    {
        task::sTaskPath path;
        path.task_id = task_id;
        path.v_control_points.clear();

        sTaskPoint point;
        if (ptr_task_process_->GetPointInfoSrv(*it, point))
        {
            path.v_key_points.push_back(point);
        }
        
        task_msg.v_task_path.push_back(path);
    }
    // printf("PathExtraction() --> %d\n", (int)task_msg.v_task_path.size());
    return true;
}